<%
if !defined?(name) || name== nil || name.empty?
  $model_name = 'usv'
else
  $model_name = name
end

if !defined?(wavefieldSize) || wavefieldSize == nil || wavefieldSize.empty?
  $wavefield_size = 1000.0
else
  $wavefield_size = wavefieldSize.to_f
end
  $wavefield_cell_count = $wavefield_size / 20.0
%>

<?xml version="1.0"?>

<sdf version='1.6'>
<!-- Basic model of the 16' WAM-V USV -->
<model name="<%= $model_name%>">
  <static>false</static>
  <link name="base_link">
    <enable_wind>true</enable_wind>
    <inertial>
      <mass>180.0</mass>
      <inertia>
        <ixx>120.0</ixx>
        <ixy>0.0</ixy>
        <ixz>0.0</ixz>
        <iyy>393</iyy>
        <iyz>0.0</iyz>
        <izz>446.0</izz>
      </inertia>
    </inertial>

    <visual name="base_visual">
      <geometry>
        <mesh><uri>meshes/WAM-V-Base.dae</uri></mesh>
      </geometry>
    </visual>

    <!-- Top base -->
    <collision name="top_base">
      <pose>0 -0 1.25 0 0 0</pose>
      <geometry>
        <box><size>1.85 1 0.1" </size></box>
      </geometry>
    </collision>

    <!-- left base -->
    <collision name="left_float">
      <pose>-0.4 1.03 0.2 0 1.57 0 </pose>
      <geometry>
        <cylinder><length>4</length><radius>0.2</radius></cylinder>
      </geometry>
    </collision>
    <collision name="left_mid_float">
      <pose>1.85 1.03 0.3 0 1.38 0</pose>
      <geometry>
        <cylinder><length>0.5</length><radius>0.17</radius></cylinder>
      </geometry>
    </collision>
    <collision name="left_front_float">
      <pose>2.3 1.03 0.4 0 1.3 0</pose>
      <geometry>
        <cylinder><length>0.45</length><radius>0.12</radius></cylinder>
      </geometry>
    </collision>

    <!-- right base -->
    <collision name="right_float">
      <pose>-0.4 -1.03 0.2 0 1.57 0 </pose>
      <geometry>
        <cylinder><length>4</length><radius>0.2</radius></cylinder>
      </geometry>
    </collision>
    <collision name="right_mid_float">
      <pose>1.85 -1.03 0.3 0 1.38 0</pose>
      <geometry>
        <cylinder><length>0.5</length><radius>0.17</radius></cylinder>
      </geometry>
    </collision>
    <collision name="right_front_float">
      <pose>2.3 -1.03 0.4 0 1.3 0</pose>
      <geometry>
        <cylinder><length>0.45</length><radius>0.12</radius></cylinder>
      </geometry>
    </collision>
  </link>

  <!-- left engine and thruster -->
  <link name="left_engine_link">
    <pose>-2.373776 1.027135 0.318237 0 0 0</pose>
    <visual name ="left_engine_visual">
      <geometry>
        <mesh><uri>meshes/engine.dae</uri></mesh>
      </geometry>
    </visual>
    <collision name="left_engine_vertical_axis_collision">
      <pose>-0.16 0 -0.24 0 0 0</pose>
      <geometry>
        <box><size>0.2 0.15 0.83</size></box>
      </geometry>
    </collision>
    <collision name="left_engine_rear_end_collision">
      <pose>-0.34 0 0.12 0 0 0</pose>
      <geometry>
        <box><size>0.12 0.15 0.12</size></box>
      </geometry>
    </collision>
    <inertial>
      <mass>15.0</mass>
      <inertia>
        <ixx>0.889245</ixx>
        <ixy>0.0</ixy>
        <ixz>0.0</ixz>
        <iyy>0.811125</iyy>
        <iyz>0.0</iyz>
        <izz>0.078125</izz>
      </inertia>
    </inertial>
  </link>
  <link name="left_propeller_link">
    <pose>-2.651932 1.027135 -0.191134 0 0 0</pose>
    <visual name ="left_propeller_visual">
      <geometry>
        <mesh><uri>meshes/propeller.dae</uri></mesh>
      </geometry>
    </visual>
    <collision name="left_propeller_collision">
      <pose>-0.08 0 0 0 1.57 0</pose>
      <geometry>
        <cylinder><length>0.18</length><radius>0.24</radius></cylinder>
      </geometry>
    </collision>
    <inertial>
      <mass>0.5</mass>
      <inertia>
        <ixx>0.008545</ixx>
        <ixy>0.0</ixy>
        <ixz>0.0</ixz>
        <iyy>0.008545</iyy>
        <iyz>0.0</iyz>
        <izz>0.0144</izz>
      </inertia>
    </inertial>
  </link>

  <joint name="left_chasis_engine_joint" type="revolute">
    <axis>
      <xyz>0 0 1</xyz>
      <limit>
        <lower>-1.57079</lower>
        <upper>1.57079</upper>
        <effort>10</effort>
        <velocity>10</velocity>
      </limit>
    </axis>
    <parent>base_link</parent>
    <child>left_engine_link</child>
  </joint>

  <joint name="left_engine_propeller_joint" type="revolute">
    <axis>
      <xyz>1 0 0</xyz>
      <limit>
        <effort>100</effort>
        <velocity>100</velocity>
      </limit>
      <dynamics>
        <friction>0.05</friction>
        <damping>0.05</damping>
      </dynamics>
    </axis>
    <parent>left_engine_link</parent>
    <child>left_propeller_link</child>
  </joint>

  <!-- right engine and thruster -->
  <link name="right_engine_link">
    <pose>-2.373776 -1.027135 0.318237 0 0 0</pose>
    <visual name ="right_engine_visual">
      <geometry>
        <mesh><uri>meshes/engine.dae</uri></mesh>
      </geometry>
    </visual>
    <collision name="right_engine_vertical_axis_collision">
      <pose>-0.16 0 -0.24 0 0 0</pose>
      <geometry>
        <box><size>0.2 0.15 0.83</size></box>
      </geometry>
    </collision>
    <collision name="right_engine_rear_end_collision">
      <pose>-0.34 0 0.12 0 0 0</pose>
      <geometry>
        <box><size>0.12 0.15 0.12</size></box>
      </geometry>
    </collision>
    <inertial>
      <mass>15.0</mass>
      <inertia>
        <ixx>0.889245</ixx>
        <ixy>0.0</ixy>
        <ixz>0.0</ixz>
        <iyy>0.811125</iyy>
        <iyz>0.0</iyz>
        <izz>0.078125</izz>
      </inertia>
    </inertial>
  </link>
  <link name="right_propeller_link">
    <pose>-2.651932 -1.027135 -0.191134 0 0 0</pose>
    <visual name ="right_propeller_visual">
      <geometry>
        <mesh><uri>meshes/propeller.dae</uri></mesh>
      </geometry>
    </visual>
    <collision name="right_propeller_collision">
      <pose>-0.08 0 0 0 1.57 0</pose>
      <geometry>
        <cylinder><length>0.18</length><radius>0.24</radius></cylinder>
      </geometry>
    </collision>
    <inertial>
      <mass>0.5</mass>
      <inertia>
        <ixx>0.008545</ixx>
        <ixy>0.0</ixy>
        <ixz>0.0</ixz>
        <iyy>0.008545</iyy>
        <iyz>0.0</iyz>
        <izz>0.0144</izz>
      </inertia>
    </inertial>
  </link>

  <joint name="right_chasis_engine_joint" type="revolute">
    <axis>
      <xyz>0 0 1</xyz>
      <limit>
        <lower>-1.57079</lower>
        <upper>1.57079</upper>
        <effort>10</effort>
        <velocity>10</velocity>
      </limit>
    </axis>
    <parent>base_link</parent>
    <child>right_engine_link</child>
  </joint>

  <joint name="right_engine_propeller_joint" type="revolute">
    <axis>
      <xyz>1 0 0</xyz>
      <limit>
        <effort>100</effort>
        <velocity>100</velocity>
      </limit>
      <dynamics>
        <friction>0.05</friction>
        <damping>0.05</damping>
      </dynamics>
    </axis>
    <parent>right_engine_link</parent>
    <child>right_propeller_link</child>
  </joint>

  <plugin
    filename="ignition-gazebo-thruster-system"
    name="ignition::gazebo::systems::Thruster">
    <joint_name>left_engine_propeller_joint</joint_name>
    <thrust_coefficient>0.004422</thrust_coefficient>
    <fluid_density>1000</fluid_density>
    <propeller_diameter>0.2</propeller_diameter>
    <velocity_control>true</velocity_control>
  </plugin>

  <plugin
    filename="ignition-gazebo-joint-position-controller-system"
    name="ignition::gazebo::systems::JointPositionController">
    <joint_name>left_chasis_engine_joint</joint_name>
    <use_velocity_commands>true</use_velocity_commands>
    <topic><%= $model_name%>/left/thruster/joint/cmd_pos</topic>
  </plugin>

  <plugin
    filename="ignition-gazebo-thruster-system"
    name="ignition::gazebo::systems::Thruster">
    <joint_name>right_engine_propeller_joint</joint_name>
    <thrust_coefficient>0.004422</thrust_coefficient>
    <fluid_density>1000</fluid_density>
    <propeller_diameter>0.2</propeller_diameter>
    <velocity_control>true</velocity_control>
  </plugin>

  <plugin
    filename="ignition-gazebo-joint-position-controller-system"
    name="ignition::gazebo::systems::JointPositionController">
    <joint_name>right_chasis_engine_joint</joint_name>
    <use_velocity_commands>true</use_velocity_commands>
    <topic><%= $model_name%>/right/thruster/joint/cmd_pos</topic>
  </plugin>

  <plugin
    filename="libSurface.so"
    name="ignition::gazebo::systems::Surface">
    <link_name>base_link</link_name>
    <vehicle_length>4.9</vehicle_length>
    <vehicle_width>2.4</vehicle_width>
    <hull_radius>0.213</hull_radius>
    <fluid_level>0</fluid_level>

    <!-- Waves -->
    <wavefield>
      <size><%= $wavefield_size%> <%= $wavefield_size%></size>
      <cell_count><%= $wavefield_cell_count%> <%=$wavefield_cell_count%></cell_count>
      <wave>
        <model>PMS</model>
        <period>5</period>
        <number>3</number>
        <scale>1.1</scale>
        <gain>0.3</gain>
        <direction>1 0</direction>
        <angle>0.4</angle>
        <tau>2.0</tau>
        <amplitude>0.0</amplitude>
        <steepness>0.0</steepness>
      </wave>
    </wavefield>
  </plugin>

  <plugin
    filename="libSimpleHydrodynamics.so"
    name="ignition::gazebo::systems::SimpleHydrodynamics">
    <link_name>base_link</link_name>
    <!-- Added mass -->
    <xDotU>0.0</xDotU>
    <yDotV>0.0</yDotV>
    <nDotR>0.0</nDotR>
    <!-- Linear and quadratic drag -->
    <xU>51.3</xU>
    <xUU>72.4</xUU>
    <yV>40.0</yV>
    <yVV>0.0</yVV>
    <zW>500.0</zW>
    <kP>50.0</kP>
    <mQ>50.0</mQ>
    <nR>400.0</nR>
    <nRR>0.0</nRR>
  </plugin>

  <!-- Publish robot state information -->
  <plugin filename="libignition-gazebo-pose-publisher-system.so"
    name="ignition::gazebo::systems::PosePublisher">
    <publish_link_pose>true</publish_link_pose>
    <publish_sensor_pose>true</publish_sensor_pose>
    <publish_collision_pose>false</publish_collision_pose>
    <publish_visual_pose>false</publish_visual_pose>
    <publish_nested_model_pose>true</publish_nested_model_pose>
    <publish_model_pose>false</publish_model_pose>
    <use_pose_vector_msg>true</use_pose_vector_msg>
    <static_publisher>true</static_publisher>
    <static_update_frequency>1</static_update_frequency>
  </plugin>

  <!-- Comms endpoint-->
  <plugin
    filename="ignition-gazebo-comms-endpoint-system"
    name="ignition::gazebo::systems::CommsEndpoint">
    <address><%= $model_name%></address>
    <topic>model/<%= $model_name%>/rx</topic>
  </plugin>

</model>
</sdf>
